/*********************************
 mando_heliroute_arma.sqf v1.3
 August 2009 Mandoble 
 
 Moves a chopper accurately to the desired destination point following the indicated route and land it if so indicated.
**********************************/

private["_heli", "_route", "_endpos", "_height", "_landing ", "_pilot", "_i", "_j", "_pos", "_dist", "_distold", "_angh", "_dir", "_accel", "_speed", "_steps", "_inipos", "_offset","_landing_pos","_z_velocity"];
_debug = (d_global_debug > 0);

if (_debug) then { 
	diag_log "";
	diag_log Format["Calling MandoHeliroute with: %1", _this];
};
_heli = _this select 0;
_route = _this select 1;
_height = _this select 2;
_landing = _this select 3;
_engineoff = True;
if (count _this > 4) then {
	_engineoff = _this select 4;
};

if (!local _heli) exitWith {hint "mando_heliroute: vehicle must be local"};

_pilot = driver _heli;
_grp = group _pilot;
_heli setVariable ["mando_heliroute", "busy",true];

_heli flyinHeight _height;
_grp setSpeedMode "LIMITED";
{
	deleteWaypoint _x;
} foreach waypoints _grp;
_heli setVelocity [0,0,0];
doStop _pilot;
if (!isEngineOn _heli) then {_pilot action ["ENGINEON", _heli]; sleep 6;};
sleep 1;
_pilot doMove [getPos _heli select 0, getPos _heli select 1, _height];
_prev_height = getPos _heli select 2;
While {getPos _heli select 2 < (_height / 2)} do {
	if (!isEngineOn _heli) then {_pilot action ["ENGINEON", _heli]; sleep 6;};
	sleep 1;
	_heli setVelocity [0,0,velocity _heli select 2];
	if (_prev_height == getPos _heli select 2) then {_pilot doMove [getPos _heli select 0, getPos _heli select 1, _height]; diag_log "moving to height";};
};
while {(!unitReady _pilot)&&(alive _pilot)&&(damage _heli < 0.5)} do {sleep 2};
if (!(alive _pilot) || (damage _heli >= 0.5)) exitWith {_heli setVariable ["mando_heliroute", "damaged",true]};
_grp setSpeedMode "FULL";
for "_j" from 0 to (count _route) - 1 do {
	_endpos = _route select _j;
	{
		deleteWaypoint _x;
	} foreach waypoints _grp;
	_inipos = getPos _heli;
	// _dist = sqrt(((_endpos select 0) - (_inipos select 0))^2 + ((_endpos select 1) - (_inipos select 1))^2);
	_dist = [_endpos, _inipos] call XfDistTo;
	_steps = _dist / 3000;
	_steps = _steps - (_steps % 1);
	_ang = ((_endpos select 0) - (_inipos select 0)) atan2 ((_endpos select 1) - (_inipos select 1));
	for "_i" from 0 to _steps - 1 do {
		_pos = [(_inipos select 0) + sin(_ang)*3000*_i,(_inipos select 1) + cos(_ang)*3000*_i, _height];
		_wp = _grp addWaypoint [_pos,(_height max 50)];
		_wp setWaypointType "MOVE";
		_grp setCurrentWaypoint _wp;
		_pilot doMove _pos;
		sleep 2;
		if (_debug) then {
			_m_marker_name = format ["steps_%1", _i];
			_marker = createMarkerLocal [_m_marker_name, _pos];
			_m_marker_name setMarkerAlphaLocal 1;
			_m_marker_name setMarkerShapeLocal "ICON";
			_m_marker_name setMarkerColorLocal "ColorRed";
			_m_marker_name setMarkerTypeLocal "hd_dot";
			_m_marker_name setMarkerTextLocal Format["Step %1",_i];
			diag_log Format["New Step: %1, Speed: %2, Limit: %3, Height: %4,  Velocity: %5, Calc: %6,  Curr: %7",(_endpos distance _heli), speed _heli, ((floor(_endpos distance _heli) / 4) + 5), (getPos _heli select 2), velocity _heli,(sqrt(((_pos select 0) - (getPos _heli select 0))^2 + ((_pos select 1) - (getPos _heli select 1))^2)), _pos];
		};

		_offset = if (_i < (_steps - 1)) then {5} else {1};

		_reset = time + 7;
		_start = time;
		_last_pos = getPos _heli;
		while {((sqrt(((_pos select 0) - (getPos _heli select 0))^2 + ((_pos select 1) - (getPos _heli select 1))^2))>500)&&(alive _pilot)&&(damage _heli < 0.5)&&((time - _start) < 200)} do {
			sleep 2;
			if (time > _reset) then {
				if (_debug) then { diag_log Format["Distance1: %1, Speed: %2, Limit: %3, Height: %4,  Velocity: %5, Calc: %6",(_endpos distance _heli), speed _heli, ((floor(_endpos distance _heli) / 4) + 5), (getPos _heli select 2), velocity _heli,(sqrt(((_pos select 0) - (getPos _heli select 0))^2 + ((_pos select 1) - (getPos _heli select 1))^2))]};
				if (((_last_pos distance (getPos _heli)) < 20) || ((speed _heli) < 0 && (speed _heli) > -30)) then {
					_heli_dir = getDir _heli;
					if (_debug) then { diag_log Format["Unblock 1: %1,  Pos:%2,  NewPos:%3,  Curr:%4",(_endpos distance _heli), (getPos _heli), _heli_dir, _pos]};
					for "_k" from 1 to 100 do {
						_vx = (sin _heli_dir) * (_k / 2);
						_vy = (cos _heli_dir) * (_k / 2);
						_vz = _k;
						if (((getPos _heli) select 2) > 500) then { _vz = ((velocity _heli) select 2); };
						_heli setVelocity [_vx,_vy,_vz];
						sleep 0.4;
					};
					_grp setCurrentWaypoint _wp;
					sleep 1;
				};
				_last_pos = getPos _heli;
				_pilot doMove _pos;
				_reset = time + 7;
			};
		};
		if (!(alive _pilot) || (damage _heli >= 0.5)) exitWith {};
	};
	if (!(alive _pilot) || (damage _heli >= 0.5)) exitWith {};

	{
		deleteWaypoint _x;
	} foreach waypoints _grp;
	if (_debug) then { diag_log Format["Distance2: %1, Speed: %2, Direction: %3, Limit: %4, Height: %5,  Velocity: %6, Pilot: %7  Dest:%8  WP:%9",(_endpos distance _heli), speed _heli, getdir _heli, ((floor(_endpos distance _heli) / 4) + 5), (getPos _heli select 2), velocity _heli, unitReady _pilot, _endpos, waypoints _grp]; };
	_wp = _grp addWaypoint [_endpos,10];
	_grp setCurrentWaypoint _wp;
	_wp setWaypointType "MOVE";
	_grp setCurrentWaypoint _wp;
	_pilot doMove _endpos;
	if (_debug) then {
		{
			diag_log Format["Mando WP: %1 %2",waypointPosition _wp, waypointSpeed _wp];
		} foreach waypoints _grp;
	};
	sleep 2;
	_reset = time + 7;
	_last_pos = getPos _heli;
	while {((!unitReady _pilot) || (abs(speed _heli) > 20))&&(alive _pilot)&&(damage _heli < 0.5) && ((_heli distance _endpos) >= 50)} do {
		sleep 0.2;
		if ((_heli distance _endpos) < 500 And (speedMode _grp != "LIMITED")) then {
			_wp setWaypointSpeed "LIMITED";
			_grp setSpeedMode "LIMITED";
		};
		if (time > _reset) then {
			if ((_last_pos distance (getPos _heli)) < 20) then {
				_heli_dir = getDir _heli;
				if (_debug) then { diag_log Format["Unblock 2: %1,  Pos:%2,  NewPos:%3,  Curr:%4",(_endpos distance _heli), (getPos _heli), _heli_dir, _pos]};
				for "_k" from 1 to 100 do {
					_vx = (sin _heli_dir) * (_k / 2);
					_vy = (cos _heli_dir) * (_k / 2);
					_vz = _k;
					if (((getPos _heli) select 2) > 500) then { _vz = ((velocity _heli) select 2); };
					_heli setVelocity [_vx,_vy,_vz];
					sleep 0.4;
				};
				_grp setCurrentWaypoint _wp;
				sleep 1;
			};
			_pilot doMove _endpos;
			_last_pos = getPos _heli;
			_reset = time + 7;
		};
		_z_velocity = (velocity _heli) select 2;
		if ((getPos _heli select 2) > _height And (_z_velocity > 0)) then { _heli setVelocity [(velocity _heli) select 0, (velocity _heli) select 1, 0.1]; };
		if (_debug) then {
			diag_log Format["Distance3: %1, Speed: %2, Direction: %3, Limit: %4, Height: %5,  Velocity: %6, Pilot: %7  Dest:%8  WPs:%9  curr:%10",(_endpos distance _heli), speed _heli, getdir _heli, ((floor(_endpos distance _heli) / 4) + 5), (getPos _heli select 2), velocity _heli, unitReady _pilot, _endpos, count (waypoints _grp), currentWaypoint _grp];
			// {
				// diag_log Format["Mando WP: %1 %2",waypointPosition _wp, waypointSpeed _wp];
			// } foreach waypoints _grp;
		};
		if ((_heli distance _endpos) < 80) then {
			doStop _pilot;
		};
	};
	if (!(alive _pilot) || (damage _heli >= 0.5)) exitWith {};
	if ((_heli getVariable "mando_heliroute") == "cancel") exitWith {_heli setVariable ["mando_heliroute", "canceled",true]};
};
// while {(abs(vectorUp _heli select 2) < 0.996)&&(alive _pilot)&&(damage _heli < 0.5)} do {
	// sleep 0.2;
	// _z_velocity = (velocity _heli) select 2;
	// if ((getPos _heli select 2) > _height And (_z_velocity > 0)) then { _heli setVelocity [(velocity _heli) select 0, (velocity _heli) select 1, 0.1]; };
// };
	
if (!(alive _pilot) || (damage _heli >= 0.5)) exitWith {_heli setVariable ["mando_heliroute", "damaged",true]};
if ((_heli getVariable "mando_heliroute") == "canceled") exitWith {_heli setVariable ["mando_heliroute", "waiting",true]};
_pilot doMove [getPos _heli select 0, getPos _heli select 1];

_dist = sqrt(((_endpos select 0) - (getPos _heli select 0))^2 + ((_endpos select 1) - (getPos _heli select 1))^2);
_dir = getDir _heli;

if ((_heli getVariable "mando_heliroute") == "canceled") exitWith {_heli setVariable ["mando_heliroute", "waiting",true]};
if (_landing) then {
	// We need to update the EndPos in case something came to block the position
	_landing_pos = [_endpos,typeof _heli,300] call XfFindLandingPosition;
	if (_debug) then { diag_log Format["Updated the Landing pos: %1 to %2 (%3)",_endpos, _landing_pos, _landing_pos distance _endpos]; };
	if ( count _landing_pos > 2) then {
		_endpos = _landing_pos;
	};
};

if ((_heli getVariable "mando_heliroute") == "canceled") exitWith {_heli setVariable ["mando_heliroute", "waiting",true]};
if (_dist > 2) then {
	_pos = _heli worldToModel _endpos;
	_angh = (_pos select 0) atan2 (_pos select 1);
	_dist = sqrt((_pos select 0)^2+(_pos select 1)^2);

	for "_i" from 0 to (abs _angh) - 1 do {
		if (!(alive _pilot) || (damage _heli >= 0.5)) exitWith {};
		_heli setDir ((getDir _heli)+abs(_angh)/_angh);
		_heli setVelocity [0,0,0];
		sleep 0.05;
	};  
};
_heli setDir (_dir + _angh);
_dir = getDir _heli;

_distold = 99999;
_dist = 99998;
_speed = 3;
while {(_distold >= _dist) && (alive _pilot) && (damage _heli < 0.5)} do {
	if (_speed < 10) then {_speed = _speed + 0.3};
	_distold = _dist;
	_dist = sqrt(((_endpos select 0) - (getPos _heli select 0))^2 + ((_endpos select 1) - (getPos _heli select 1))^2);
	_heli setVelocity [sin(_dir)*_speed, cos(_dir)*_speed, 0];
	_heli setDir _dir;
	sleep 0.001;
};

if (!(alive _pilot) || (damage _heli >= 0.5)) exitWith {_heli setVariable ["mando_heliroute", "damaged",true]};
if ((_heli getVariable "mando_heliroute") == "canceled") exitWith {_heli setVariable ["mando_heliroute", "waiting",true]};

if (_landing) then {
	_b_landed = False;
	_i_time_out = 0;
	// diag_log "Landing started";
	_speed = -7;
	while {(isEngineOn _heli) && (alive _pilot) && (damage _heli < 0.5) && (! _b_landed) && (_i_time_out < 12000)} do {
		if (((getPos _heli select 2) > ((_endpos select 2) + 2)) And (_i_time_out < 6000)) then {
			_heli setDir _dir;
			if ((getPos _heli select 2) > ((_endpos select 2) + 10)) then {
				_speed = 0 - (((getPos _heli select 2) / 7) max 7);
				_heli setVelocity [0,0,-7];
			} else {
				_heli setVelocity [0,0,4*((((getPos _heli select 2)-(_endpos select 2))/-5) min -1)];
			};
			// diag_log "Descending";
		} else {
			if (_engineoff) then {
				_pilot action ["ENGINEOFF", _heli];
			} else {
				_b_landed = True;			
			};
			_heli setVelocity [0,0,0];
			// diag_log "Landing";
		};
		sleep 0.08;
	};
	if (_debug) then { diag_log "Landing end"; };

	for "_i" from 0 to 199 do {
		_heli setVelocity [0,0,-0.6];
		sleep 0.02;
	};
	if (_debug) then { diag_log "Landed"; };

	if (!(alive _pilot) || (damage _heli >= 0.5)) exitWith {_heli setVariable ["mando_heliroute", "damaged",true]};
	if ((_heli getVariable "mando_heliroute") == "canceled") exitWith {_heli setVariable ["mando_heliroute", "waiting",true]};

	_heli setVariable ["mando_heliroute", "waiting",true];
	if (_debug) then { diag_log "start waiting"; };
	_pilot stop true;
	while {(alive _pilot) && (damage _heli < 0.5) && ((_heli getVariable "mando_heliroute") == "waiting")} do {
		_pilot action ["ENGINEOFF", _heli];
		//if (!_engineoff) then {_pilot action ["ENGINEON", _heli];};
		_heli setVelocity [0,0,-0.1];
		sleep 0.01;
	};
	if (_debug) then { diag_log "done waiting"; };
	if (!(alive _pilot) || (damage _heli >= 0.5)) exitWith {_heli setVariable ["mando_heliroute", "damaged",true]};
	if ((_heli getVariable "mando_heliroute") == "cancel") exitWith {_heli setVariable ["mando_heliroute", "canceled",true]};
} else {
	_heli setVariable ["mando_heliroute", "waiting",true];
};

if (!(alive _pilot) || (damage _heli >= 0.5)) exitWith {_heli setVariable ["mando_heliroute", "damaged",true]};